within OpenMBS.Bodies;

/*partial model RigidBody*/
model RigidBody

	 parameter Real[3] r_MC_abs_0 = zeros(3);
	 parameter Real[3] v_MC_abs_0 = zeros(3);
	 parameter Real[3] a_MC_abs_0 = zeros(3);
	 /*parameter Real[3] quaternion_0 = ;*/
	 parameter Real[3] angular_vel_abs_0 = zeros(3);
	 parameter Real[3] angular_acc_abs_0 = zeros(3);
	 parameter Real[3,3] T_0 = [1, 0, 0; 
	 							0, 1, 0;
								0, 0, 1]; 

	outer OpenMBS.Static.World world;
	/*replaceable KinematicPort OutPort;*/

	parameter Real m=1 "Mass of the body";
	parameter Real I[3, 3]=[1, 0, 0; 0, 1, 0; 0, 0, 1] "Body's central tensor of inertia";

	Real r_MC_abs[3] (start = r_MC_abs_0)  "Radius vector of MC_abs";
	Real v_MC_abs[3] (start = v_MC_abs_0)  "Velocity vector of MC_abs";
	Real a_MC_abs[3] (start = a_MC_abs_0)  "Acceleration vector of MC_abs";

	Real quaternion[4] "Quaternion of body orientation";
	Real q[4]; //helper var to make equations more readable
	Real angular_vel_abs[3] (start = angular_vel_abs_0)  "Vector of angular rate";
	Real angular_acc_abs[3] (start = angular_acc_abs_0, stateSelect = StateSelect.never) "Vector of angular acceleration";

	Real T[3, 3] (start = T_0) "Matrix of rotation from Abs to Body?";

	Real F[3] "Sum of all forces applied";
	Real M[3] "Sum of all torques applied";
	Real Active(start=1) "Flag of active dynamics";

protected
	// These are used to calculate der(q).
	// All serve for q3, and q3 - for der(q).
	Real a1;
	Real a2;
	Real A1[3];
	Real A2[3];
	Real A3[3];
	Real q3[4];

equation
	
	der(Active) = 0;
	der(r_MC_abs) = Active*v_MC_abs;
	der(v_MC_abs) = Active*a_MC_abs;

	m*a_MC_abs = F;

	q = quaternion;

	// calculate der(q)
	a1 = q[1];
	A1 = {q[2],q[3],q[4]};
	a2 = 0;
	A2 = {angular_vel_abs[1],angular_vel_abs[2],angular_vel_abs[3]};
	A3 = a1*A2 + a2*A1 + cross(A1, A2);
	q3 = {a1*a2 - A1*A2,A3[1],A3[2],A3[3]};

	der(q) = Active*0.5*q3;
	//  der(q) = Active*0.5*QMult(q, {0,angular_vel_abs[1],angular_vel_abs[2],angular_vel_abs[3]});
	der(angular_vel_abs) = Active*angular_acc_abs;

	T = [q[1]^2 + q[2]^2 - q[3]^2 - q[4]^2, 2*(q[2]*q[3] - q[1]*q[4]), 2*(q[2]*q[4] + q[1]*q[3]);
		 2*(q[1]*q[4] + q[2]*q[3]), q[1]^2 - q[2]^2 + q[3]^2 -q[4]^2, 2*(q[3]*q[4] - q[1]*q[2]);
		 2*(q[2]*q[4] - q[1]*q[3]), 2*(q[1]*q[2] + q[3]*q[4]), q[1]^2 - q[2]^2 - q[3]^2 + q[4]^2]
		 /(q*q);
	/*T = T_0;*/

	I*angular_acc_abs + cross(angular_vel_abs, I*angular_vel_abs) = transpose(T)*M;

	/*F = zeros(3);*/
	F = T * world.gravity;
	M = zeros(3);
/*
	OutPort.r = r_MC_abs;
	OutPort.v = v_MC_abs;
	OutPort.a = a_MC_abs;
	OutPort.T = T;
	OutPort.angular_vel_abs = T*angular_vel_abs;
	OutPort.angular_acc_abs = T*angular_acc_abs;
*/
end RigidBody;
